#include "define.h"


struct DMC_DATA dmc = DEFAULT_DMC;
Uint16 mode = 0;

//10 53 01 00 启动电机
//10 52 84 03 转速900rpm

int main(void)
{
	Init();
	
	EPwm1Regs.ETCLR.bit.INT = 1U;
	EPwm1Regs.TBCTR = 0U;
	EPwm1Regs.ETSEL.bit.INTEN = 1;

	while(1)
	{
		if(g_SysData.PeriodFlg == 1U)
		{
			IM();
			IFBIT();
			OM();
			g_SysData.PeriodFlg = 0U;
		}
	}
}


void IM(void)
{

	AnaInput();
}
void OM(void)
{
	static Uint16 uiCnt = 0U;

	CanDataPack();

	uiCnt++;
	if( (uiCnt >= (g_CanMsg.CommPeriod / 20) ) && (g_CanMsg.CommPeriod != 0) )
	{
		uiCnt = 0U;
		MailBox0Tx();
	}
}

/*-------------------------------------------------------------------------------
 * Function > kev_bool Interpolate(kev_float32 x1, kev_float32 x2, kev_float32 y1, kev_float32 y2, kev_float32 xResult, kev_float32 *yResult)
 * Description > 插值
 * Version > V0.00
 * Input >
 * Output > 无。
 * Return > 是否成功
 * Author > Wenxinfeng
 * Date > 2018.04.01
 * History >
 *-------------------------------------------------------------------------------*/
Uint16 Interpolate(float32 x1, float32 x2, float32 y1, float32 y2, float32 xResult, float32 *yResult)
{
    float32 temp1 = 0.F;
    float32 temp2 = 0.F;
    Uint16 rtnValue = 1;

    if(fabsf(x2 - x1) < 0.0001F)
    {
        rtnValue = 0;
    }
    else
    {
        temp1 = (y2 - y1) / (x2 - x1);
        temp2 = ((y1 * x2) - (y2 * x1)) / (x2 - x1);
        *yResult = (xResult * temp1) + temp2;
    }

    return rtnValue;
}


#define MotorU_50V  			400.F
#define MotorU_100V  			805.F
#define MotorU_150V  			1220.F
#define MotorU_200V  			1610.F
#define MotorU_250V  			2000.F
#define MotorU_250V_300V_K  	50 / (MotorU_250V - MotorU_200V)
#define MotorU_250V_300V_B  	250 - MotorU_250V_300V_K * MotorU_250V

#define MotorI_0mA  			400.F
#define MotorI_130mA  			700.F
#define MotorI_200mA  			870.F
#define MotorI_360mA  			1280.F
#define MotorI_480mA  			1550.F
#define MotorI_630mA  			1950.F
#define MotorI_630mA_3A_K  		150 / (MotorI_630mA - MotorI_480mA)
#define MotorI_630mA_3A_B  		630 - MotorI_630mA_3A_K * MotorI_630mA

void CanDataPack(void)
{
	float tempMotorV = 0.F;
	float tempMotorI = 0.F;
	//can data package
	if( (g_CanMsg.ErrState.bit.SpeedOver == 1) || (g_CanMsg.ErrState.bit.MotorIOver == 1) )
	{
		//g_CanMsg.Control.bit.MotorEn = 0;
	}

//	g_CanMsg.MotorV = (float32)g_SysData.AnalogData.Items.MOTOR_U * 0.108 - 23;// * 2.2f * 3051 / 51 / 1000;
	if(g_SysData.AnalogData.Items.MOTOR_U < MotorU_50V)
	{
		 Interpolate(0.F, MotorU_50V, 0.F, 50.F, (float)g_SysData.AnalogData.Items.MOTOR_U, &tempMotorV);
		 g_CanMsg.MotorV = tempMotorV;
	}else if(g_SysData.AnalogData.Items.MOTOR_U < MotorU_100V)
	{
		Interpolate(MotorU_50V, MotorU_100V, 50.F, 100.F, (float)g_SysData.AnalogData.Items.MOTOR_U, &tempMotorV);
		g_CanMsg.MotorV = tempMotorV;
	}
	else if(g_SysData.AnalogData.Items.MOTOR_U < MotorU_150V)
	{
		Interpolate(MotorU_100V, MotorU_150V, 100.F, 150.F, (float)g_SysData.AnalogData.Items.MOTOR_U, &tempMotorV);
		g_CanMsg.MotorV = tempMotorV;
	}
	else if(g_SysData.AnalogData.Items.MOTOR_U < MotorU_200V)
	{
		Interpolate(MotorU_150V, MotorU_200V, 150.F, 200.F, (float)g_SysData.AnalogData.Items.MOTOR_U, &tempMotorV);
		g_CanMsg.MotorV = tempMotorV;
	}
	else if(g_SysData.AnalogData.Items.MOTOR_U < MotorU_250V)
	{
		Interpolate(MotorU_200V, MotorU_250V, 200.F, 250.F, (float)g_SysData.AnalogData.Items.MOTOR_U, &tempMotorV);
		g_CanMsg.MotorV = tempMotorV;
	}
	else{
		g_CanMsg.MotorV = MotorU_250V_300V_K * (float)g_SysData.AnalogData.Items.MOTOR_U + MotorU_250V_300V_B;
	}






//	g_CanMsg.MotorA = (Uint32)g_SysData.AnalogData.Items.MOTOR_I *4.9 - 1778;// * 950U / 2920U;
	if(g_SysData.AnalogData.Items.MOTOR_I < MotorI_0mA)
	{
		g_CanMsg.MotorA = 0;
	}else if(g_SysData.AnalogData.Items.MOTOR_I < MotorI_130mA)
	{
		 Interpolate(MotorI_0mA, MotorI_130mA, 0.F, 130.F, (float)g_SysData.AnalogData.Items.MOTOR_I, &tempMotorI);
		 g_CanMsg.MotorA = tempMotorI;
	}else if(g_SysData.AnalogData.Items.MOTOR_I < MotorI_200mA)
	{
		Interpolate(MotorI_130mA, MotorI_200mA, 130.F, 200.F, (float)g_SysData.AnalogData.Items.MOTOR_I, &tempMotorI);
		g_CanMsg.MotorA = tempMotorI;
	}
	else if(g_SysData.AnalogData.Items.MOTOR_I < MotorI_360mA)
	{
		Interpolate(MotorI_200mA, MotorI_360mA, 200.F, 360.F, (float)g_SysData.AnalogData.Items.MOTOR_I, &tempMotorI);
		g_CanMsg.MotorA = tempMotorI;
	}
	else if(g_SysData.AnalogData.Items.MOTOR_I < MotorI_480mA)
	{
		Interpolate(MotorI_360mA, MotorI_480mA, 360.F, 480.F, (float)g_SysData.AnalogData.Items.MOTOR_I, &tempMotorI);
		g_CanMsg.MotorA = tempMotorI;
	}
	else if(g_SysData.AnalogData.Items.MOTOR_I < MotorI_630mA)
	{
		Interpolate(MotorI_480mA, MotorI_630mA, 480.F, 630.F, (float)g_SysData.AnalogData.Items.MOTOR_I, &tempMotorI);
		g_CanMsg.MotorA = tempMotorI;
	}
	else{
		g_CanMsg.MotorA = MotorI_630mA_3A_K * (float)g_SysData.AnalogData.Items.MOTOR_I + MotorI_630mA_3A_B;
	}




	g_CanMsg.SpeedFeed = dmc.fSpdFb * BASE_RPM;
	g_CanMsg.DrvState = 0;
	g_CanMsg.MotorOverI = (int32)VAL_MOTOR_I_LMT * 950U / 2920U;
	g_CanMsg.MotorUnderV = (int32)VAL_MOTOR_U_LMT  * 2.2f * 3051 / 1000 / 51;
	g_CanMsg.Temperate = (int32)g_SysData.AnalogData.Items.Temperate / -6 + 317;
}



void MotorCtl(void)
{
    //高压电源启动后允许接通电机
    if(g_CanMsg.ErrState.bit.MotorUUnder == 0)
    {
        dmc.uiEn = g_CanMsg.Control.bit.MotorEn;
    }
    else
    {
        dmc.uiEn = 0;
    }

	if(g_CanMsg.Control.bit.MotorDir == 0)
	{
		dmc.fSpdRef = g_CanMsg.SpeedRef / BASE_RPM;
	}
	else
	{
		dmc.fSpdRef = g_CanMsg.SpeedRef / BASE_RPM * -1.F;
	}


	//dmc.uiRdcRaw = ((*(Uint16 *)ADD_RDC_POS) & 0XFFFFUL) >> 4;
	dmc.uiRdcRaw = RDCPosGet();
	DmcTestMode(&dmc, mode);

	EPwm1Regs.CMPA.half.CMPA = (Uint16)(VAL_EPWM_PRD * dmc.fTa);
	EPwm3Regs.CMPA.half.CMPA = (Uint16)(VAL_EPWM_PRD * dmc.fTb);
	EPwm2Regs.CMPA.half.CMPA = (Uint16)(VAL_EPWM_PRD * dmc.fTc);


	if(dmc.uiEn == 1)
	{
		MotorON();
	}
	else
	{
		MotorOFF();
	}
}




void MotorOFF (void)
{
	DO_MOTOR_EN = 1;

	EPwm1Regs.AQCSFRC.all = 6;
	EPwm2Regs.AQCSFRC.all = 6;
	EPwm3Regs.AQCSFRC.all = 6;

    return;
}
void MotorON (void)
{
	DO_MOTOR_EN = 0;

	EPwm1Regs.AQCSFRC.all = 0;
	EPwm2Regs.AQCSFRC.all = 0;
	EPwm3Regs.AQCSFRC.all = 0;

    return;
}


void PluseGen(void)
{
	static int16 RdcLast = 0;   //上一拍的RDC值
	static Uint16 First = 0;    //首次调用
	static Uint16 LastVal = 0;  //上一拍的HALL

	//首次进入时的初始化代码
	if(First == 0)
	{
		First = 1;
		RdcLast = ((*(Uint16 *)ADD_RDC_POS) & 0XFFFFUL) >> 4;
		g_PlusGen.Base = RdcLast;
	}

	//读取RDC数据
    g_PlusGen.CurrentRdc = ((*(Uint16 *)ADD_RDC_POS) & 0XFFFFUL) >> 4;

	DO_PLUSE_RPM = DI_HALL1;//将HALL引脚电平转发出去
	//检测到HALL下降沿后，重置5°脉冲计数，调整5°基准值为当前RDC数值，重置RDC圈数
	if( (DI_HALL1 == 0) && (LastVal == 1) )//fall edge detect and zero correct
	{
		g_PlusGen.PluseCnt = 0;
		g_PlusGen.Base = g_PlusGen.CurrentRdc;
		g_PlusGen.RdcRoundCnt = 0;
	}
	LastVal = DI_HALL1;

	//RDC当前读数 + RDC圈数 * 4096 + 1        大于          5°基准值 + 5°脉冲计数 * 传动比（RDC读数到输出轴）
	if( ((int32)g_PlusGen.CurrentRdc + g_PlusGen.RdcRoundCnt * 4096 + 1) > ((int32)g_PlusGen.Base + (int32)((float32)g_PlusGen.PluseCnt * 278.3F) ) )
	{
		DO_PLUSE = 1;
		DELAY_US(1);
		DO_PLUSE = 0;
		g_PlusGen.PluseCnt++;
	}

	//RDC过零点检测，RDC圈数累计。
	if( ((g_PlusGen.CurrentRdc - RdcLast) > 2048) || ((g_PlusGen.CurrentRdc - RdcLast) < -2048) )
	{
		g_PlusGen.RdcRoundCnt++;
	    //hall error and manual zero correct
	    if(g_PlusGen.RdcRoundCnt > 0x000FFFFF)
	    {
	        g_PlusGen.PluseCnt = 0;
	        g_PlusGen.Base = g_PlusGen.CurrentRdc;
	        g_PlusGen.RdcRoundCnt = 0;
	    }
	}
    RdcLast = g_PlusGen.CurrentRdc;
}

float32 AnaCheck(Uint16 uiData)
{
    float32 fAnalog = 0.F;

    fAnalog = ((float32)((uiData >> 4) * 3U)) * 1000 / 4096.F;

    return fAnalog;
}
Uint16 GetAverage( Uint16 *data , Uint16 num )
{
	Uint16 max = 0U;
	Uint16 min = 0U;
	Uint16 sum = 0U;
	Uint16 iLoop = 0U;

	if( num == 2u )//2锟斤拷值时锟斤拷去锟斤拷锟斤拷锟叫�
	{
		sum = (data[0] + data[1]) / 2;
	}
	else if ( num < 2U )	//锟斤拷锟斤拷小锟斤拷2锟斤拷锟斤拷锟斤拷锟剿诧拷
	{
		sum = data[0];
	}
	else
	{
		max = data[0];
		min = data[0];
		sum = data[0];
		for( iLoop = 1U; iLoop < num; iLoop++ )
		{
			if( max < data[iLoop] )
			{
				max = data[iLoop];//寻锟斤拷锟斤拷锟街�
			}
			else if( min > data[iLoop] )
			{
				min = data[iLoop]; //寻锟斤拷锟斤拷小值
			}
			else{}
			sum = sum + data[iLoop];
		}
		sum = ( (sum - min - max) / (num - 2U) );
	}

	return sum;
}

void AnaInput(void)
{
	static Uint16 jLocation = 0U;
    Uint16 uiLoop = 0U;
	static Uint16 s_AnaBuf[VAL_ANA_SIZE][20] = {
													{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
													{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
													{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
													{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
													{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}
												};

    for(uiLoop = 0U; uiLoop < VAL_ANA_SIZE; uiLoop++)
    {
    	s_AnaBuf[uiLoop][jLocation] = AnaCheck(AdcRegs.ADCRESULT[uiLoop]);
    	g_SysData.AnalogData.ItemArray[uiLoop] = GetAverage( s_AnaBuf[uiLoop] , 20U );
    }
	jLocation++;
	if( jLocation == 20U )
	{
		jLocation = 0U;
	}
    return;
}
